#include "TDBase.h"

PathFollower::PathFollower(void)
	:_pPool(NULL)
{
}

PathFollower::~PathFollower(void)
{
	ReleasePath();
}

void PathFollower::Init (PathRecyclePool* pPool, int pathIdx, vector3di& start, int segIdx)
{
	assert (pPool);
	assert (pathIdx>=0 && pathIdx<pPool->Size());
	assert (segIdx >=0 && segIdx <pPool->At(pathIdx).Size());

	ReleasePath();
	_pPool = pPool;
	_pathIdx =pathIdx;

	_start.X = (float)start.X;
	_start.Y = (float)start.Y;
	_start.Z = (float)start.Z;
	_segIdx = segIdx;
	_distance = 0.0f;
	_posi = _start;

	InitSegment(false);
}

void PathFollower::InitSegment(bool updateStart)
{
	if (updateStart)
	{
		const vector3di& start = Path().VertAt(_segIdx-1);
		_start.X = (float)start.X;
		_start.Y = (float)start.Y;
		_start.Z = (float)start.Z;
	}

	const vector3di& end = Path().VertAt(_segIdx);
	_end.X = (float)end.X;
	_end.Y = (float)end.Y;
	_end.Z = (float)end.Z;

	_segLen =_start.getDistanceFrom(_end);
	_direction = _end - _start;
}

bool PathFollower::Forward (float distance)
{
	assert (_pPool);
	assert (_pathIdx>=0 && _pathIdx<_pPool->Size());
	assert (distance >= 0.0f);

	_distance += distance;

	while (_distance >= _segLen)
	{
		if (_segIdx == Path().Size() -1)
		{
			_posi = _end;
			return true;
		}

		_distance -= _segLen;
		_segIdx ++;
		InitSegment(true);
	}

	_posi = _start +_direction * (_distance / _segLen);

	return false;
}

const vector3df& PathFollower::Position() const
{
	return _posi;
}

const vector3df& PathFollower::Direction() const
{
	return _direction;
}

const MapPath& PathFollower::Path()
{
	assert(_pPool);
	return _pPool->At(_pathIdx);
}

void PathFollower::ReleasePath()
{
	if (_pPool)
	{
		_pPool->ReleasePath (_pathIdx);
		_pPool = NULL;
	}
}